/*************************************************************************
 Projet  -  Sport Collectif pour ControleRobots
 -------------------
 Auteur  -  Kevin Vandel
 *************************************************************************/

//- Realisation de la classe <ControleRobot> (fichier ControleRobot.cpp) -

//---------------------------------------------------------------- INCLUDE

//-------------------------------------------------------- Include systeme
#include <iostream>
#include <unistd.h>
#include <cmath>

using namespace std;

//------------------------------------------------------ Include personnel
#include "ControleRobot.h"


//----------------------------------------------------------------- PUBLIC

//----------------------------------------------------- Methodes publiques
bool ControleRobot::initCom()
{
	return (comKhepera.Ouvrir(usbPort)
			&& comKhepera.ConfigurerPort(Communication::CCOM_BR9600, Communication::CCOM_8_BITS,
										 Communication::CCOM_NONE, Communication::CCOM_1_BIT));
} //----- Fin de <InitCom>


bool ControleRobot::tourner(Vec2d directionObj, positionPostRotation posRot)
// Algorithme :
//	On determine si il faut tourner et si oui dans quel sens.
//	On calcule la distance a parcourir par les roues pour la rotation.
//	On scrute la vitesse du robot, et on sort de la methode quand le robot a fini de tourner.
{
	LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Direction objectif : " << directionObj[X] << ", " << directionObj[Y]);

	// On recupere la direction effective du robot
	Position posControleRobot;
	getPosition(posControleRobot);
	Vec2d directionEff = (posRot == ARRIERE ? -posControleRobot.orientation : posControleRobot.orientation);
	LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Direction robot : " << directionEff[X] << ", " << directionEff[Y]);

	// Pour savoir si il faut tourner, on calcule le produit scalaire entre les
	// directions effective et voulue (equivalant au cosinus de l'angle) :
	double prodScal = directionEff.dot(directionObj);

	// Si cette composante est proche de zero, on considere que les axes sont alignes
	// et donc qu'il n'est pas utile de tourner
	if ( prodScal > SEUIL_ORIENTATION || (posRot == OPTIMALE && -prodScal > SEUIL_ORIENTATION) )
	{
		LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Bon alignement du robot");
		return true;
	}

	// sinon on doit tourner

	double angle; //Angle de rotation a parcourir
	resetPositionRoues(); //RAZ des positions des roues

	// Pour determiner dans quel sens le robot doit tourner, on calcule en plus la composante
	// verticale produit vectoriel(equivalant au sinus de l'angle) :
	double zProdVect = directionEff[X] * directionObj[Y] - directionEff[Y] * directionObj[X];

	// Gestion de l'orientation finale souhaitee :
	if( posRot == OPTIMALE ) //Cas le plus complexe
	{
		angle = abs(asin(zProdVect)); //Angle compris entre 0 et 90 degres
		int position = (int) ((angle * CONST_ROTATION / 2) + 0.5); //Position a parcourir par chaque roue

		// Si la composante du produit vectoriel et le produit scalaire sont de signes differents,
		// il faut tourner a droite (et sinon a gauche)
		if ((zProdVect > 0) ^ (prodScal > 0)) //On utilise l'operateur XOR
		{
			LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Rotation de " << angle*180/M_PI << " degres a droite");
			comKhepera.SetPositionRoues(position, -position); // tourner a droite
		}
		else
		{
			LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Rotation de " << angle*180/M_PI << " degres a gauche");
			comKhepera.SetPositionRoues(-position, position); // tourner a gauche
		}
	}
	else //Orientation souhaitee : marche avant ou marche arriere
	{
		angle = acos(prodScal); //Angle compris entre 0 et 180 degres
		int position = (int) ((angle * CONST_ROTATION / 2) + 0.5); //Position a parcourir par chaque roue

		// Si le produit vectoriel est positif, il faut tourner a gauche (sinon a droite)
		if( zProdVect > 0 )
		{
			LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Rotation de " << angle*180/M_PI << " degres a gauche");
			comKhepera.SetPositionRoues(-position, position); // tourner a gauche
		}
		else
		{
			LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Rotation de " << angle*180/M_PI << " degres a droite");
			comKhepera.SetPositionRoues(position, -position); // tourner a droite
		}
	}

	// On scrute la vitesse du robot
	while (iAQueue->isEmpty() && isControleThreadRunning())
	{
		// On endort le thread pour avoir un frequence de controle inferieure ou egale
		// au rafraichissement du module de vision :
		usleep(PERIODE_CONTROLE);

		pair<int, int> vitesses = comKhepera.GetVitesseRoues();
		pair<int, int> positions = comKhepera.GetPositionRoues();
		if(abs(positions.first) > 10000 || abs(positions.second) > 10000) break;

		if( (vitesses.first == 0) && (vitesses.second == 0) ) //Robot immobile
		{
			LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Fin de rotation");
			return true;
		}
	}

	// Si la rotation a ete interrompue :
	comKhepera.SetPositionRoues(0,0);
	resetPositionRoues();
	return false;
} //----- Fin de <tourner>


bool ControleRobot::avancer(double distance, bool marcheAvant)
{
	int positionRoues = (int) (distance * CM_TO_PULSE);
	resetPositionRoues();

	if (marcheAvant)
	{
		LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Deplacement marche avant sur " << distance << " cm");
		comKhepera.SetPositionRoues(positionRoues, positionRoues); //On avance
	}
	else
	{
		LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Deplacement marche arriere sur " << distance << " cm");
		comKhepera.SetPositionRoues(-positionRoues, -positionRoues); //On recule
	}

	// On scrute la vitesse du robot
	bool finDeplacement = false;
	while (!finDeplacement  && iAQueue->isEmpty() && isControleThreadRunning())
	{
		// On endort le thread pour avoir une frequence de controle inferieure ou egale
		// au rafraichissement du module de vision :
		usleep(PERIODE_CONTROLE);

		pair<int, int> vitesses = comKhepera.GetVitesseRoues();
		pair<int, int> positions = comKhepera.GetPositionRoues();
		if(abs(positions.first) > 10000 || abs(positions.second) > 10000) break;

		if( (vitesses.first == 0) && (vitesses.second == 0) ) //Robot immobile
		{
			LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Fin de deplacement");
			finDeplacement = true;
		}
	}
	// Si le deplacement a ete interrompu :
	if(!finDeplacement)
	{
		comKhepera.SetPositionRoues(0,0);
		resetPositionRoues();
	}
	return finDeplacement;
} //----- Fin de <avancer>


void ControleRobot::tirer(bool rotationGauche)
{
	resetPositionRoues();
	if (rotationGauche)
	{
		LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Tir vers la gauche");
		comKhepera.SetVitesseRoues(-VITESSE_BOOST, VITESSE_BOOST); //Rotation sur la gauche
	}
	else
	{
		LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Tir vers la droite");
		comKhepera.SetVitesseRoues(VITESSE_BOOST, -VITESSE_BOOST); //Rotation sur la droite
	}

	// On arrete le "boost" du robot au bout d'une duree predefinie
	usleep(DUREE_TIR);
	arreterRobot();
} //----- Fin de <tirer>


bool ControleRobot::arreterRobot()
{
	LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Arret du robot");
	return comKhepera.SetVitesseRoues(0, 0);
} //----- Fin de <arreterRobot>


bool ControleRobot::LancerControle()
{
	if(controleThreadLoop)
	{
		//le thread est deja lance
		LOG4CPLUS_WARN(loggerControle, "Le thread est déjà lancé (return).");
		return false;
	}
	controleThreadLoop = true;

	LOG4CPLUS_INFO(loggerControle, "[R" << idRob << "] Lancement Controle");

	if(!initCom())
	{
		//echec de l'initialisation du port de communication
		return false;
	}

	//lancer le thread de lecture de la position du robot
	visionParams = new val_st<ControleRobot>;
	visionParams->pThis = this;
	visionParams->pFunc = this->visionThreadWrapper();
	if( pthread_create(&visionThreadId, NULL, &pThreadWrapper<ControleRobot>, static_cast<void*>(visionParams)) != 0 )
	{
		//echec de la creation du thread
		return false;
	}

	//lancer le thread de controle du robot
	controlerParams = new val_st<ControleRobot>;
	controlerParams->pThis = this;
	controlerParams->pFunc = this->controlerThreadWrapper();
	if( pthread_create(&controlerThreadId, NULL, &pThreadWrapper<ControleRobot>, static_cast<void*>(controlerParams)) != 0 )
	{
		//echec de la creation du thread
		return false;
	}

#ifdef ASSERVISSEMENT_ACTIF
	//lancer le thread d'asservissement de la trajectoire du robot
	asservirParams = new val_st<ControleRobot>;
	asservirParams->pThis = this;
	asservirParams->pFunc = this->asservirThreadWrapper();
	if( pthread_create(&asservirThreadId, NULL, &pThreadWrapper<ControleRobot>, static_cast<void*>(asservirParams)) != 0 )
	{
		//echec de la creation du thread
		return false;
	}
#endif

	return true;
} //----- Fin de <Launch>


int ControleRobot::ArreterControle()
{
	if(!controleThreadLoop)
	{
		//le thread est deja lance
		return false;
	}

	pthread_mutex_lock(&lockControleThreadLoop);
	controleThreadLoop = false;
	pthread_mutex_unlock(&lockControleThreadLoop);

	DataVision dataVis;
	memset( &dataVis, 0, sizeof(DataVision) );
	visionQueue->push(dataVis);
	DataIA dataIA;
	memset( &dataVis, 0, sizeof(DataIA) );
	iAQueue->push(dataIA);

	LOG4CPLUS_INFO(loggerControle, "[R" << idRob << "] Arret Controle | Attendre la terminaison des threads.");

	//Attendre la terminaison des threads
	pthread_join(visionThreadId, NULL);
	pthread_join(controlerThreadId, NULL);
#ifdef ASSERVISSEMENT_ACTIF
	pthread_join(asservirThreadId, NULL);
#endif

	LOG4CPLUS_INFO(loggerControle, "[R" << idRob << "] ArreterControle | Threads terminés.");

	// Envoi d'un ordre d'arrêt au robot
	arreterRobot();

	// Nettoyage
	delete visionParams;
	delete controlerParams;
#ifdef ASSERVISSEMENT_ACTIF
	delete asservirParams;
#endif

	return 0;
} //----- Fin de <ArreterControle>


//-------------------------------------------- Constructeurs - destructeur

ControleRobot::ControleRobot(string port, int numeroRob, MsgQueue<DataVision>* _visionQueue, MsgQueue<DataIA>* _iAQueue)
{
	LOG4CPLUS_TRACE(loggerControle, "Appel au constructeur de <ControleRobot>");

	this->usbPort = port;
	this->idRob = numeroRob;
	this->visionQueue = _visionQueue;
	this->iAQueue = _iAQueue;
	controleThreadLoop = false;
	if ( 	pthread_mutex_init(&lockVisionData, NULL) != 0 ||
			pthread_mutex_init(&lockIaData, NULL) != 0 ||
			pthread_mutex_init(&lockExit, NULL) != 0 ||
			pthread_cond_init(&asservissement, NULL) != 0 ||
			pthread_cond_init(&arrive, NULL) != 0 ||
			pthread_mutex_init(&lockAsservissement, NULL) != 0 ||
			pthread_mutex_init(&lockArrive, NULL) != 0 ||
			pthread_mutex_init(&lockControleThreadLoop, NULL) != 0 )
	{
		LOG4CPLUS_INFO(loggerControle, "Constructeur de <ControleRobot> : erreur init objet de synchronisation.");
	}
} //----- Fin de ControleRobot


ControleRobot::~ControleRobot()
{
	LOG4CPLUS_TRACE(loggerControle, "Appel au destructeur de <ControleRobot>");

	pthread_mutex_destroy(&lockVisionData);
	pthread_mutex_destroy(&lockIaData);
	pthread_mutex_destroy(&lockExit);
	pthread_cond_destroy(&asservissement);
	pthread_cond_destroy(&arrive);
	pthread_mutex_destroy(&lockAsservissement);
	pthread_mutex_destroy(&lockArrive);
	pthread_mutex_destroy(&lockControleThreadLoop);
} //----- Fin de ~ControleRobot



//---------------------------------------------------------------- PROTEGE

//----------------------------------------------------- Methodes protegees

bool ControleRobot::resetPositionRoues()
{
	LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Reset positions roues");
	return comKhepera.SetCompteurPositionRoues(0, 0);
} //----- Fin de <resetPositionRoues>


bool ControleRobot::isControleThreadRunning()
{
	bool cr;
	pthread_mutex_lock(&lockControleThreadLoop);
	cr = controleThreadLoop;
	pthread_mutex_unlock(&lockControleThreadLoop);
	return cr;
} //----- Fin de <isControleThreadRunning>


void ControleRobot::visionThread()
{
	DataVision data;
	while (isControleThreadRunning())
	{
		visionQueue->wait_and_pop(data);
		setVisionData(data);
	}
} //----- Fin de <visionThread>


void ControleRobot::controlerThread()
{
	Position posControleRobot;
	Vec2d coordDest;	//Coordonnees du point a atteindre
	Vec2d coordRobot;	//Coordonnes du robot
	Vec2d directionDest;//Vecteur normalise (position robot, position objectif)
	DataIA dataIA;		//Objectif donne par l'IA
	char strategie;		//Strategie definie par l'IA
	double distance;	//Distance du robot a son objectif
	double prodScal;	//Produit scalaire entre les directions effective et voulue

	while( isControleThreadRunning() )
	{
		// On recupere un nouvel objectif :
		iAQueue->wait_and_pop(dataIA);
		LOG4CPLUS_DEBUG(loggerControle, "[R" << idRob << "] Nouvel objectif recu");
		setIaData(dataIA);
		coordDest = dataIA.posObjectif;
		strategie = dataIA.typeStrategie;


		getPosition(posControleRobot);
		coordRobot = posControleRobot.coord;

		setExit(true);

		if(strategie == DEFENSE)
		{
			// On suppose que le robot est en position de défense, il doit juste avancer ou reculer
			// pour aller chercher la balle

			// Vecteur destination :
			directionDest = coordDest - coordRobot;
			distance = norm(directionDest);
			directionDest[X] /= distance; //Normalisation
			directionDest[Y] /= distance; //Normalisation

			getPosition(posControleRobot);
			coordRobot = posControleRobot.coord;
			distance = norm(coordDest - coordRobot); //Distance du robot a son objectif

			// Determination du sens de marche
			prodScal = posControleRobot.orientation.dot(directionDest);
			marcheAvant = prodScal > 0;

			// On tient compte de la longueur du bras lors des interceptions
			//distance = (marcheAvant ? distance - DELTA_DEFENSE : distance + DELTA_DEFENSE);

			//if( !avancer(distance, marcheAvant) ) continue;

			if (marcheAvant)
			{
				// si le robot doit aller en marche avant, il n'est pas obligé de bouger si la raquette est assez longue pour intercepter la balle
				if(distance > LONGUEUR_BRAS)
				{
					if( !avancer(distance-LONGUEUR_BRAS, marcheAvant) )
						continue;
				}
			}
			else
			{
				if( !avancer(distance, marcheAvant) )
					continue;
			}

			/*
			// Pour avoir une plus grande precision, on renvoie l'ordre au
			//robot pour qu'il se positionne au plus pres de l'objectif
			getPosition(posControleRobot);
			double distanceObj;
			distanceObj = sqrt((posControleRobot.coord[X]-coordDest[X])*(posControleRobot.coord[X]-coordDest[X]) + (posControleRobot.coord[Y]-coordDest[Y])*(posControleRobot.coord[Y]-coordDest[Y]));
			if(distanceObj > MARGE_SECURITE)
			{
				// On revoie le message
				iAQueue->push(dataIA);
				continue;
			}
			*/
		}

		// Implementation des differentes strategies :
		else if( strategie == REPLACEMENT )
		{
			// Vecteur destination :
			directionDest = coordDest - coordRobot;
			distance = norm(directionDest);
			directionDest[X] /= distance; //Normalisation
			directionDest[Y] /= distance; //Normalisation

			// Rotation du robot :
			if( !tourner(directionDest) ) continue;

			// Une fois le robot bien oriente, on verifie si le robot doit avancer ou reculer
			// en calculant le produit scalaire entre les directions effective et voulue :
			getPosition(posControleRobot);
			coordRobot = posControleRobot.coord;
			distance = norm(coordDest - coordRobot); //Distance du robot a son objectif

			// Determination du sens de marche
			prodScal = posControleRobot.orientation.dot(directionDest);
			marcheAvant = prodScal > 0;

			if( !avancer(distance, marcheAvant) ) continue;

			// Pour avoir une plus grande precision, on renvoie l'ordre au
			//robot pour qu'il se positionne au plus pres de l'objectif
			getPosition(posControleRobot);
			double distanceObj;
			distanceObj = sqrt((posControleRobot.coord[X]-coordDest[X])*(posControleRobot.coord[X]-coordDest[X]) + (posControleRobot.coord[Y]-coordDest[Y])*(posControleRobot.coord[Y]-coordDest[Y]));
			if(distanceObj > MARGE_SECURITE)
			{
				// On revoie le message
				iAQueue->push(dataIA);
				continue;
			}

			// Orientation optimale du robot pour intercepter la balle plus rapidement
			if( !tourner(dataIA.dirObjectif, AVANT) ) continue;
		}
		else //Phase d'attaque
		{
			// Positionnement du robot :
			//On decompose le deplacement en deux, selon les Y puis les X
			//afin de limiter les risques de collision avec la balle.

			// Deplacement en Y :
			distance = abs(coordDest[Y] - coordRobot[Y]);
			directionDest[X] = 0;
			directionDest[Y] = (coordDest[Y] - coordRobot[Y]) / distance; //Normalisation
			//On force le robot a s'orienter en marche arriere (pour eviter de taper nos buts avec le bras)
			marcheAvant = false;
			if( !tourner(directionDest, ARRIERE) ) continue;
			if( !avancer(distance, marcheAvant) ) continue;


			// Deplacement en X :
			getPosition(posControleRobot);
			coordRobot = posControleRobot.coord;
			distance = abs(coordDest[X] - coordRobot[X]);
			directionDest[X] = (coordDest[X] - coordRobot[X]) / distance; //Normalisation
			directionDest[Y] = 0;
			//On force le robot a s'orienter en marche arriere (pour eviter de taper la balle avec le bras)
			if( !tourner(directionDest, ARRIERE) ) continue;
			if( !avancer(distance, marcheAvant) ) continue;

			// Pour avoir une plus grande precision, on renvoie l'ordre au
			//robot pour qu'il se positionne au plus pres de l'objectif
			getPosition(posControleRobot);
			double distanceObj;
			distanceObj = sqrt((posControleRobot.coord[X]-coordDest[X])*(posControleRobot.coord[X]-coordDest[X]) + (posControleRobot.coord[Y]-coordDest[Y])*(posControleRobot.coord[Y]-coordDest[Y]));
			if(distanceObj > MARGE_SECURITE)
			{
				// On revoie le message
				iAQueue->push(dataIA);
				continue;
			}

			// On regarde dans quel sens, il faut tirer
			Position balle;
			getPosition(posControleRobot);
			getPositionBalle(balle);

			// Tir
			tirer( posControleRobot.coord[X] < balle.coord[X] );
		}

		setExit(false);
		pthread_cond_signal(&asservissement);
	}
} //----- Fin de <controlerThread>


void ControleRobot::asservirThread()
{/*
	// Controle du deplacement du robot : asservissement + verification arrivee
	double erreur, errSom, errDif = 0.0; //Initialisation des erreurs
	double erreurBase, correctif;
	int vitGaucheInit, vitDroiteInit = 0; //Vitesse initiale des roues
	double vitGauche, vitDroite; //Vitesse corrigees des roues
	Vec2d coordControleRobot;
	Vec2d coordDest;
	Vec2d directionControleRobot;
	Vec2d directionDest;
	int distance; //Distance entre le robot et sa destination
	Position posControleRobot;
	Position posObjectifRobot;

	while (isControleThreadRunning())
	{
		pthread_mutex_lock(&lockAsservissement);
		pthread_cond_wait(&asservissement,&lockAsservissement);
		pthread_mutex_unlock(&lockAsservissement);
		do
		{
			// On endort le thread pour avoir un frequence de controle inferieure
			// au rafraichissement du module de vision :
			usleep(PERIODE_ASSERVISSEMENT);

			getPosition(posControleRobot);
			getObjPosition(posObjectifRobot);
			coordControleRobot = posControleRobot.coord;
			coordDest = posObjectifRobot.coord;
			distance = norm(coordControleRobot - coordDest);

			// On se sert du produit vectoriel comme erreur de base (il doit etre idealement nul)
			directionControleRobot = posControleRobot.orientation;
			directionDest = coordDest-coordControleRobot;
			double norme = norm(directionDest);
			// Normalisation du vecteur destination :
			directionDest[X] /= norme;
			directionDest[Y] /= norme;
			erreurBase = directionControleRobot[X] * directionDest[Y] - directionControleRobot[Y] * directionDest[X];

			errDif = erreurBase - erreur;	//Composante Differentielle : variation de l'erreur
			erreur = erreurBase;			//Composante Proportionnelle : erreur brute
			errSom += erreur;				//Composante Integrale : somme des erreurs depuis le debut

			correctif = (erreur * COEF_P) + (errSom * COEF_I) + (errDif * COEF_D);

			vitGauche = comKhepera.GetVitesseRoues().first;
			vitDroite = comKhepera.GetVitesseRoues().second;

			int vitTotal;
			if(marcheAvant){ vitTotal = 20;}
			if(!marcheAvant){ vitTotal = -20;}

			// Correction des vitesses :
			// Si le correctif est positif et que le robot avance (ou ni l'un ni l'autre)
			// il faut tourner plus a gauche (sinon plus a droite)
			//prodScal = directionControleRobot.dot(directionDest);
			//marcheAvant = prodScal > 0;
			if ((correctif > 0.0) ^ marcheAvant) //On utilise l'operateur XOR
			{
				vitGauche -= correctif / 2 + 0.5;
				vitDroite += correctif / 2 + 0.5;
			}
			else
			{
				vitGauche += correctif / 2 + 0.5;
				vitDroite -= correctif / 2 + 0.5;
			}

			if(marcheAvant && (vitGauche + vitDroite != vitTotal))
			{
				vitGauche = vitGauche * vitTotal / (vitGauche + vitDroite);
				vitDroite = vitDroite * vitTotal / (vitGauche + vitDroite);
				if(vitGauche < 6){ vitGauche = 6;}
				if(vitGauche > 14){ vitGauche = 14;}
				if(vitDroite < 6){ vitDroite = 6;}
				if(vitDroite > 14){ vitDroite = 14;}
			}
			if(!marcheAvant && (vitGauche + vitDroite != vitTotal))
			{
				vitGauche = vitGauche * vitTotal / (vitGauche + vitDroite);
				vitDroite = vitDroite * vitTotal / (vitGauche + vitDroite);
				if(vitGauche < -6){ vitGauche = -6;}
				if(vitGauche > -14){ vitGauche = -14;}
				if(vitDroite < -6){ vitDroite = -6;}
				if(vitDroite > -14){ vitDroite = -14;}
			}

			if ((int)(vitGauche) != vitGaucheInit || (int)(vitDroite) != vitDroiteInit)
			{
				vitGaucheInit = (int)(vitGauche);
				vitDroiteInit = (int)(vitDroite);
				comKhepera.SetVitesseRoues(vitGaucheInit, vitDroiteInit);
			}
		}
		while (distance > SEUIL_DISTANCE && !isExit() && isControleThreadRunning()); //tant que destination non atteinte et pas de nouvel objectif
		arreterRobot();
		if(distance > SEUIL_DISTANCE)
		{
			pthread_cond_signal(&arrive);
		}
	}*/
	//return NULL;
} //----- Fin de <asservirThread>


void ControleRobot::getPosition(Position& posRob)
{
	pthread_mutex_lock(&lockVisionData);
	posRob = visionData.posRobots[idRob];
	pthread_mutex_unlock(&lockVisionData);
} //----- Fin de <getPosition>

void ControleRobot::getPositionBalle(Position& posBalle)
{
	pthread_mutex_lock(&lockVisionData);
	posBalle = visionData.posBalle;
	pthread_mutex_unlock(&lockVisionData);
} //----- Fin de <getPosition>


void ControleRobot::setVisionData(DataVision data)
{
	pthread_mutex_lock(&lockVisionData);
	visionData = data;
	pthread_mutex_unlock(&lockVisionData);
} //----- Fin de <setVisionData>


void ControleRobot::getObjPosition(Position& posObjRob)
{
	pthread_mutex_lock(&lockIaData);
	posObjRob.coord = iaData.posObjectif;
	posObjRob.orientation = iaData.dirObjectif;
	pthread_mutex_unlock(&lockIaData);
} //----- Fin de <getObjPosition>


void ControleRobot::setIaData(DataIA data)
{
	pthread_mutex_lock(&lockIaData);
	iaData = data;
	pthread_mutex_unlock(&lockIaData);
} //----- Fin de <setIaData>


bool ControleRobot::isExit()
{
	bool cr;
	pthread_mutex_lock(&lockExit);
	cr = exit;
	pthread_mutex_unlock(&lockExit);
	return cr;
} //----- Fin de <isExit>


void ControleRobot::setExit(bool b)
{
	pthread_mutex_lock(&lockExit);
	exit = b;
	pthread_mutex_unlock(&lockExit);
} //----- Fin de <setExit>
